#include "common/se2.h"

using namespace std;

SE2::SE2() : so2_(SO2()), p_(Eigen::Vector2d::Zero()) {

}

SE2::SE2(const double& x, const double& y, const double& t) : so2_(SO2(t)), p_(Eigen::Vector2d(x, y)) {

}

SE2::SE2(const SO2& so2, const Eigen::Vector2d& p) : so2_(so2), p_(p) {

}

SE2 SE2::Inv() const {
    // double c = std::cos(so2_.log());
    // double s = std::sin(so2_.log());
    double c = so2_.matrix()(0, 0);  
    double s = so2_.matrix()(1, 0);

    double x = p_[0];
    double y = p_[1];
    Eigen::Vector2d p_inv = Eigen::Vector2d(-c*x-s*y, s*x-c*y);
    // return SE2(-so2_.log(), p_inv);
    return SE2(so2_.inv(), p_inv);  // 修复：使用SO2的inv方法
}

// Same as: that.inv() + *this
SE2 SE2::operator +(const SE2& that) const{
    // double theta = SO2(so2_).log();
    // double that_theta = SO2(that.so2_).log();

    // double c = std::cos(theta);
    // double s = std::sin(theta);
    // double x = p_[0] + that.p_[0]*c - that.p_[1]*s;
    // double y = p_[1] + that.p_[0]*s + that.p_[1]*c;
    // SO2 t = SO2(theta + that_theta);
    // return SE2(t, Eigen::Vector2d(x, y));

    Eigen::Matrix2d R = so2_.matrix();
    Eigen::Matrix2d that_R = that.so2_.matrix();

    Eigen::Vector2d p_result = p_ + R * that.p_;
    SO2 t_result = so2_ * that.so2_;  // 修复：使用SO2的乘法
    
    return SE2(t_result, p_result);
}

SE2 SE2::operator -(const SE2& that) const{
    // Eigen::Vector2d dp = p_-that.p_;
    // double dx = dp[0];
    // double dy = dp[1];

    // SO2 dt = so2_- SO2(that.so2_);
    // double dth = dt.log();

    // float c = std::cos(SO2(that.so2_).log());
    // float s = std::sin(SO2(that.so2_).log());
    // return SE2(c*dx+s*dy, -s*dx+c*dy, dth);

    Eigen::Matrix2d that_R_transpose = that.so2_.matrix().transpose();
    Eigen::Vector2d dp = that_R_transpose * (p_ - that.p_);

    // 修复：创建SO2对象来进行运算
    SO2 inv_that_so2 = SO2(that.so2_.inv());  // 将double转换为SO2
    SO2 dt = inv_that_so2 * so2_;  // 现在都是SO2类型，可以相乘
    double dth = dt.log();

    return SE2(dp[0], dp[1], dth);
}

Eigen::Matrix3d SE2::matrix() const {
    Eigen::Matrix3d p = Eigen::Matrix3d::Identity();
    p.block<2,2>(0,0) = so2_.matrix();
    p.block<2,1>(0,2) = p_;
    return p;
}

Eigen::Matrix4d SE2::matrix4d(const SO2& so2, const Eigen::Vector2d& p) {
    Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
    pose.block<2,2>(0,0) = SO2(so2).matrix();
    pose.block<2,1>(0,3) = p;
    return pose;
}

Eigen::Matrix3d SE2::skew(const Eigen::Vector3d& p) {
    Eigen::Matrix3d skew_p;
    // skew_p << 0, -p.z(), p.x(),
    //           p.z(), 0, p.y(),
    //           -p.x(), -p.y(), 0;

    skew_p << 0, -p.z(), p.y(),    // 修复：修正反对称矩阵定义
              p.z(), 0, -p.x(),
              -p.y(), p.x(), 0;
    return skew_p;
}